import matplotlib.pyplot as plt

be_data_time = []
be_data_x = []
be_data_y = []
be_data_p = []
af_data_time = []
af_data_x = []
af_data_y = []
af_data_p = []
sobe_data_time = []
sobe_data_x = []
sobe_data_y = []
sobe_data_p = []
soaf_data_time = []
soaf_data_x = []
soaf_data_y = []
soaf_data_p = []


def time_conversion(r_data):
    time = int(r_data[3]) * 3600 + int(r_data[4]) * 60 + int(r_data[5]) + float(r_data[6]) / 1000
    return time


be_file = open('radar_data.txt', 'r')
for data in be_file:
    beforeSet = data.split(',')  # 对于每一行，按','把数据分开
    be_data_time.append(time_conversion(beforeSet))
    x = float(beforeSet[10])
    y = float(beforeSet[11])
    p = (x ** 2 + y ** 2) ** 0.5
    be_data_x.append(x)
    be_data_y.append(y)
    be_data_p.append(p)
print('配准前——时间', be_data_time)
print('配准前——x坐标', be_data_x)
print('配准前——y坐标', be_data_y)

af_file = open('radar_synced_data.txt', 'r')
for line in af_file:
    afterSet = line.split(',')  # 对于每一行，按','把数据分开
    if afterSet[7] != 'None\n':
        af_data_time.append(time_conversion(afterSet))
        x = float(afterSet[10])
        y = float(afterSet[11])
        p = (x ** 2 + y ** 2) ** 0.5
        af_data_x.append(x)
        af_data_y.append(y)
        af_data_p.append(p)
print('配准后——时间', af_data_time)
print('配准后——x坐标', af_data_x)
print('配准后——y坐标', af_data_y)

sobe_file = open('sound_data.txt', 'r')
for data in sobe_file:
    beforeSet = data.split(',')  # 对于每一行，按','把数据分开
    sobe_data_time.append(time_conversion(beforeSet))
    x = float(beforeSet[8])
    y = float(beforeSet[9])
    p = (x ** 2 + y ** 2) ** 0.5
    sobe_data_x.append(x)
    sobe_data_y.append(y)
    sobe_data_p.append(p)
print('配准前——时间', sobe_data_time)
print('配准前——x坐标', sobe_data_x)
print('配准前——y坐标', sobe_data_y)

soaf_file = open('sound_synced_data.txt', 'r')
for line in soaf_file:
    afterSet = line.split(',')  # 对于每一行，按','把数据分开
    if afterSet[7] != 'None\n':
        soaf_data_time.append(time_conversion(afterSet))
        x = float(afterSet[8])
        y = float(afterSet[9])
        p = (x ** 2 + y ** 2) ** 0.5
        soaf_data_x.append(x)
        soaf_data_y.append(y)
        soaf_data_p.append(p)
print('配准后——时间', soaf_data_time)
print('配准后——x坐标', soaf_data_x)
print('配准后——y坐标', soaf_data_y)

plt.rcParams['font.sans-serif'] = ['SimHei']
plt.plot(be_data_time, be_data_p, 'g.', label="雷达时间配准前")
plt.plot(af_data_time, af_data_p, 'r.', label="雷达时间配准后")
plt.plot(sobe_data_time, sobe_data_p, 'c.', label="声源时间配准前")
plt.plot(soaf_data_time, soaf_data_p, 'b.', label="声源时间配准后")

for i in range(len(af_data_time)):
    t = af_data_time[i]
    r_p = af_data_p[i]
    s_p = soaf_data_p[i]
    if r_p >= s_p:
        plt.plot([t, t], [-0.5, r_p], 'y--')
    elif r_p < s_p:
        plt.plot([t, t], [-0.5, s_p], 'y--')

plt.xlabel("时间戳（距凌晨相对时间）")
plt.ylabel("目标位置（距原点）")
plt.ylim(-0.5, 5)
plt.legend()
plt.show()
